#include <stdio.h>
#include <avr/pgmspace.h>
#include "can_avr.h"
#include "em36.h"
#include "body.h"
#include "lcd.h"



#define TIMEOUT	(5000/100)				//5000ms at 100ms rate


uint8_t BODY_status = 0;

uint8_t BODY_CAN_count = 0;

uint8_t BODY_flags = 0;
uint8_t BODY_ambient = 0;
uint8_t BODY_ADC[5];
uint8_t BODY_PortA, BODY_PortB, BODY_PortC, BODY_PortD;

stDateTime_t BODY_time = { 0, 0, 0, 0, 0, 0 };

mobCtrl_t rxMsg401;
mobCtrl_t txMsg501;

extern bool bTestMode;


static uint16_t _timeout;


void BODY_init( void )
{
	CAN_objRx( &rxMsg401, 8, 0x401, 8 );
	txMsg501.dlc = 4;
	txMsg501.msgId = 0x501;
	txMsg501.mob = 9;
	BODY_flags = 0;
	BODY_status = 0;
	BODY_CAN_count = 0;
	_timeout = TIMEOUT;
}


void BODY_setTime( stDateTime_t *pTime )
{
	txMsg501.data[0] = 0x00;		//set time
	txMsg501.data[1] = pTime->hour;
	txMsg501.data[2] = pTime->minute;
	txMsg501.data[3] = pTime->second;
	CAN_objTxOnce( &txMsg501 );
}


void BODY_setDate( stDateTime_t *pTime ) 
{
	txMsg501.data[0] = 0x01;		//set time
	txMsg501.data[1] = pTime->year;
	txMsg501.data[2] = pTime->month;
	txMsg501.data[3] = pTime->day;
	CAN_objTxOnce( &txMsg501 );	
}


void BODY_task100ms( void )
{
	uint8_t subIdx;	
	uint8_t i;

	if( CAN_objStatus(&rxMsg401) == MOB_Complete ) {

		_timeout = TIMEOUT;
		BODY_CAN_count++;
		
		BODY_flags = rxMsg401.data[0];
		//data[1] - unknown
		subIdx = rxMsg401.data[2];
		
		switch( subIdx ) {
		case 00:
			//date
			BODY_time.year = rxMsg401.data[3];
			BODY_time.month = rxMsg401.data[4];
			BODY_time.day = rxMsg401.data[5];	
			BODY_status |= BODY_STAT_DATE;
			break;
		case 01:
			//time of day
			BODY_time.hour = rxMsg401.data[3];		// hours
			BODY_time.minute = rxMsg401.data[4];	// minutes
			BODY_time.second = rxMsg401.data[5];	// seconds
			BODY_status |= BODY_STAT_TIME;
			break;
		case 02:
			BODY_ambient = rxMsg401.data[3];
			BODY_PortA = rxMsg401.data[4];			// raw port inputs (PINA)
			BODY_PortB = rxMsg401.data[5];
			BODY_PortC = rxMsg401.data[6];
			BODY_PortD = rxMsg401.data[7];
			break;
		case 03:
			BODY_ADC[0] = rxMsg401.data[3];
			BODY_ADC[1] = rxMsg401.data[4];
			BODY_ADC[2] = rxMsg401.data[5];
			BODY_ADC[3] = rxMsg401.data[6];
			BODY_ADC[4] = rxMsg401.data[7];
			break;
		}
	}
	
	if( _timeout ) {
		_timeout--;
	} else {
		//we've recieved no messages
		if( bTestMode ) {
			BODY_flags++;
			BODY_ambient++;
			for( i = 0; i < 5; i++ ) BODY_ADC[i]++;
			BODY_PortA++;
			BODY_PortB++;
			BODY_PortC++;
			BODY_PortD++;
		}
		else {
			BODY_status = 0;
			BODY_flags = BODY_SIDE | BODY_ERROR;
		}
	}
}

